#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <opencv2/opencv.hpp>

void callback(const sensor_msgs::LaserScan::ConstPtr& scan)
{ 
    cv::Mat img(500,500,CV_8UC1,255);
    double resolution = 20.0; //一米20个像素

    for(size_t i =0;i<scan->ranges.size();i++)
    {
        if(scan->ranges[i]< 0.1 || scan->ranges[i]>18)
        {
            continue;
        }

        double real_angle = scan->angle_min + i*scan->angle_increment;
        double x = scan->ranges[i]*cos(real_angle);
        double y = scan->ranges[i]*sin(real_angle);
        int image_x = int(x*resolution+500.f/2.f);
        int image_y = int(500.f/2.f-y*resolution);

        if(image_x>=0 && image_x<500 && image_y>=0 && image_y<500)
        {
            img.at<int>(image_y,image_x) = 0;
        }
    }

    cv::imshow("scan",img);
    cv::waitKey(1);

}


int main(int argc,char** argv)
{
    ros::init(argc, argv, "scan2png");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/pavo_scan_bottom", 1, callback);

    ros::spin();
    return 0;
}